#include "Bsp.h"

struct PWM Pwm = {150,150};
struct angle Angle = {0,0,0};
extern uint8_t g_current_mode;



/**
 * @brief 归一化角度到[-180, 180)区间
 * @param angle 输入角度值
 * @return 归一化后的角度
 */
float NormalizeAngle(float angle) {
    // 使用循环加减360度，直到角度在[-180, 180)之间
    while (angle > 180.0f) {
        angle -= 360.0f;
    }
    while (angle <= -180.0f) {
        angle += 360.0f;
    }
    return angle;
}

/**
 * @brief 检测计数器奇偶变化并更新角度值
 * @param offset 角度偏移量
 * @param isMode3 是否为模式3的特殊处理
 * @note 当计数器从奇数变为偶数时更新PID.Median
 */
void UpdateAngleOnEvenTransition(float offset, bool isMode3) 
{
    static int lastCount = -2;  // 初始值确保首次不匹配
    
    if(counter.count != lastCount) {
        // 检测奇->偶变化（使用位运算提高效率）
        if((counter.count & 1) == 0 && (lastCount & 1) != 0) {
            float angleZhong;
            
            if (isMode3) {
                // 模式3：基于当前角度减去180度，然后向左偏转指定角度
                // 相当于从-180°位置开始向左偏转
                angleZhong = Angle.start - 180.0f + offset;
                
                // 特殊边界处理确保连续性
                if (angleZhong > 180.0f) {
                    angleZhong -= 360.0f;
                } else if (angleZhong < -180.0f) {
                    angleZhong += 360.0f;
                }
            } else {
                // 模式2：直接从当前角度减去偏移量
                angleZhong = Angle.start - offset;
            }
            
            // 应用角度归一化
            PID.Median = NormalizeAngle(angleZhong);
        }
        lastCount = counter.count;  // 更新状态
    }
}

/**
 * @brief 执行模式特定的角度更新
 * @note 根据g_current_mode选择不同的角度偏移量
 */
void ExecuteAngleUpdate(void)
{
    switch(g_current_mode) {
        case 2:  // 模式2使用182.8f偏移
            UpdateAngleOnEvenTransition(182.4f, false);
            break;
            
        case 3:  // 模式3使用40.0f偏移（基于-180°位置向左偏转40°）
            UpdateAngleOnEvenTransition(43.5f, true);
            break;
            
        default: // 其他模式不需要角度更新
            break;
    }
}

/**
 * @brief 通用电机控制任务
 * @param maxCount 最大有效计数值
 */
void MotorControlTask(int maxCount)
{
    // 超出计数范围时停止电机
    if(counter.count > maxCount) {
        Motor_Stop();
        return;
    }

    // 电机控制逻辑
    switch(counter.count) {
        case 0:
        case 2:  // 直行控制模式
            Pwm.L = SpeedL_Pwm() - Angle_PID();
            Pwm.R = SpeedR_Pwm() + Angle_PID();
            break;
            
        case 1:
        case 3:  // 转向控制模式
            Pwm.L = SpeedL_Pwm() - Turn_PID();
            Pwm.R = SpeedR_Pwm() + Turn_PID();
            break;
            
        default: // 其他情况保持停止
            Motor_Stop();
            return;
    }
    Set_Pwm(Pwm.L, Pwm.R);
}

struct RXgle
{
    int kaiting;
    int mode;

};

struct RXgle Rxgle={0};

// 精简后的任务函数
void Task_1(void) {
    MotorControlTask(0);  // 最大计数为0
    (counter.count == 1) ? Sg() : 0 ;
}

void Task_2(void) {
    MotorControlTask(3);  // 最大计数为3
    
}

void Task_3(void) {
    
    (Rxgle.kaiting == 0) ? MotorControlTask(3) : 0;
}

void Task_4(void) {
    (Rxgle.kaiting == 0) ? MotorControlTask(3) : 0;
}

// 优化后的中断处理函数
void Pid_Time_INST_IRQHandler(void) 
{
    static uint32_t timer_counter = 0;
    static uint32_t Timer_count = 0;
    static uint32_t counter_count_prev; // 移除初始化
    static bool pid_changed = false;
    static bool initialized = false;    // 新增标志，用于首次初始化
    static uint8_t current_mode = 0;    // 跟踪当前模式
    
    if(DL_TimerG_getPendingInterrupt(Pid_Time_INST) == DL_TIMER_IIDX_ZERO) 
    {
        // 1. 首次进入时初始化counter_count_prev
        if (!initialized) {
            counter_count_prev = counter.count;
            initialized = true;
        }
        
        // 2. 保存当前模式，用于后续判断模式变化
        const uint8_t mode = g_current_mode;
        if (mode != current_mode) {
            // 模式变化时重置计时器和状态
            timer_counter = 0;
            if (pid_changed) {
                Get_Pid(120.6, 0.6, 0, 15, 302, 0, 0.520, 302, 0, 0.520, 88.7, 0.0, 1.0);
                pid_changed = false;
            }
            current_mode = mode;
        }
        
        // 3. 执行模式特定的角度更新
        ExecuteAngleUpdate();
        
        // 4. 执行当前模式的任务
        static void (*tasks[])(void) = {0, Task_1, Task_2, Task_3, Task_4};
        
        if(mode > 0 && mode < sizeof(tasks)/sizeof(tasks[0])) {
            tasks[mode]();
        }
        
        // 5. 检测counter.count变化 (优先处理)
        if (mode > 2 && counter.count != counter_count_prev) 
        {
            // 重置计时器和状态
            timer_counter = 0;
            Rxgle.kaiting = 0 ;
            // 恢复初始PID参数
            if (pid_changed) {
                Get_Pid(120.6, 0.6, 0, 15, 302, 0, 0.520, 302, 0, 0.520, 88.7, 0.0, 1.0);
                pid_changed = false;
            }
            
            counter_count_prev = counter.count;
        }
        
        // 6. 计时逻辑
        if (g_current_mode > 2) 
        {
            (!(counter.count % 2)) ? timer_counter++ : 0 ;
            if (counter.count == 0) 
            {
                // 2秒后修改PID参数 (20ms * 100 = 2秒)
                if ((timer_counter == 85 && !pid_changed)) 
                {    
                    PID.Median = (Angle.start < -150.0f) ? (360.0f + Angle.start - 34.0f)  : (Angle.start - 34.0f);
                    Get_Pid(50.6, 0.6, 0, 10, 152, 0, 0.220, 152, 0, 0.220, 127.7, 0.0, 0.1);
                    pid_changed = true;
                }
                
                if ((timer_counter == 165 && pid_changed)) 
                {   
                    Rxgle.kaiting = 1 ;
                    Set_Pwm(1400, 2800);
                } 
            }
            
            if (counter.count == 2)
            {
                if (timer_counter == 65 && !pid_changed)
                {
                    UpdateAngleOnEvenTransition(42.0f,true);
                    Get_Pid(70.6, 0.6, 0, 10, 152, 0, 0.220, 152, 0, 0.220, 127.7, 0.0, 0.1);
                    pid_changed = true;
                }
                if ((timer_counter == 125 && pid_changed))
                {   
                    Rxgle.kaiting = 1 ;
                    Set_Pwm(2900, 1200);
                }      
            } 
                if ((counter.count % 2))
                {   Get_Pid(120.6, 0.6, 0, 15, 302, 0, 0.520, 302, 0, 0.520, 88.7, 0.0, 1.0);
                    pid_changed = false;
                    timer_counter = 0;
                }
        }
        else 
        {
            // 模式不为>2时重置计时器和状态
            timer_counter = 0;
            if (pid_changed) {
                Get_Pid(120.6, 0.6, 0, 15, 302, 0, 0.520, 302, 0, 0.520, 88.7, 0.0, 1.0);
                pid_changed = false;
            }
        }
    }
    if ((g_current_mode == 2)&&(counter.count==2)) 
    {
        Timer_count++;
        if(Timer_count == 45)
        {
           Get_Pid(50.6, 0.6, 0, 10, 152, 0, 0.220, 152, 0, 0.220, 127.7, 0.0, 0.1); 
        }
        if(Timer_count == 70)
        {
            Timer_count = 0;
            Get_Pid(120.6, 1.3, 0, 18, 362, 0, 1.820, 362, 0, 1.820, 130.7, 0.0, 2.6);
        }
    }
}

void Set_Angle(void)
{
    // 更新起始角度
    Angle.start = SAngle.Angle[2];
    
    // 根据当前模式处理中值计算
    if (g_current_mode < 3) {
        // 模式1和2直接使用起始角度
        PID.Median = Angle.start;
    } else {
        // 模式3及以上使用偏移计算
        PID.Median = (Angle.start < -140.0f) 
                     ? (360.0f + Angle.start - 38.0f) 
                     : (Angle.start - 38.0f);
    }
}

void Start_Pid(void)
{
    (g_current_mode == 1) ? Get_Pid(200, 2.4, 0, 20, 500, 0, 1.1, 500, 0, 1.1, 0, 0, 0) : 0 ;
    (g_current_mode == 2) ? Get_Pid(120, 1.3, 0, 18, 360, 0, 2.0, 360, 0, 2.0, 98.7, 0, 1.3) : 0;
    (g_current_mode == 3) ? Get_Pid(130.0, 1.8, 0, 14, 350, 0, 0.520, 350, 0, 0.520, 68.7, 0.0,1.0 ) : 0;
}